// core of the Physics engine. Defines various classes we will
// need to use for mathematical modelling of the game's physics

#pragma once

#include <d3dx9.h>
#include <d3d9.h>
#include <math.h>
#include "Precision.h"

namespace physics {
	///////////////////////////////////////////
	// SleepEpsilon
	//
	// used to verify if an object hasn't moved in a while and 
	// needs to fall asleep
	///////////////////////////////////////////
	extern real sleepEpsilon;
	void setSleepEpsilon(real value);
	real getSleepEpsilon();

	///////////////////////////////////////////
	// Vector3
	///////////////////////////////////////////
	// holds a 3D vector object, with three allocated pieces of data. The fourth is padding to ensure
	// the object is 8 bytes large (32-bit word)
	class Vector3
	{
	public:
		real x;
		real y;
		real z;
	private:
		real padding; // private so we can't accidentally use this for anything

	public:
		//////////////////////////////////////////////
		// default vector is zero
		//////////////////////////////////////////////
		Vector3() : x(0.0f), y(0.0f), z(0.0f) {}

		//////////////////////////////////////////////
		// set the implicit vector provided
		//////////////////////////////////////////////
		Vector3(const real x, const real y, const real z) : x(x), y(y), z(z) {}

		//////////////////////////////////////////////
		// set the vector from the provided D3DXVECTOR3 object
		//////////////////////////////////////////////
		Vector3(D3DXVECTOR3 vector) : x(vector.x), y(vector.y), z(vector.z) {}

		//////////////////////////////////////////////
		// flips all of the vector components
		//////////////////////////////////////////////
		void invert()
		{
			x = -x;
			y = -y;
			z = -z;
		}

		//////////////////////////////////////////////
		// Gets the magnitude (length) of the vector
		//////////////////////////////////////////////
		real magnitude() const
		{
			return sqrt(x*x + y*y + z*z);
		}

		//////////////////////////////////////////////
		// Gets the square magnitude of the vector (sqrt is processor-heavy
		// and so this is useful for optimization in some calculations that
		// use length-squared)
		//////////////////////////////////////////////
		real squareMagnitude() const
		{
			return (x*x + y*y + z*z);
		}

		//////////////////////////////////////////////
		// Normalize the vector to keep it's direction but
		// set to a unit length
		//////////////////////////////////////////////
		void normalize()
		{
			real length = magnitude();
			if (length > 0)
			{
				(*this) *= 1.0f / length;
			}
		}

		//////////////////////////////////////////////
		// Overload *=
		//////////////////////////////////////////////
		void operator*=(const real value)
		{
			x *= value;
			y *= value;
			z *= value;
		}

		//////////////////////////////////////////////
		// Overload *
		//////////////////////////////////////////////
		Vector3 operator*(const real value) const
		{
			return Vector3(x*value, y*value, z*value);
		}

		//////////////////////////////////////////////
		// Overload += 
		//////////////////////////////////////////////
		void operator+=(const Vector3& vec)
		{
			x += vec.x;
			y += vec.y;
			z += vec.z;
		}

		//////////////////////////////////////////////
		// Overload +
		//////////////////////////////////////////////
		Vector3 operator+(const Vector3& vec) const
		{
			return Vector3(x+vec.x, y+vec.y, z+vec.z);
		}

		//////////////////////////////////////////////
		// Overload -= 
		//////////////////////////////////////////////
		void operator-=(const Vector3& vec)
		{
			x -= vec.x;
			y -= vec.y;
			z -= vec.z;
		}

		//////////////////////////////////////////////
		// Overload -
		//////////////////////////////////////////////
		Vector3 operator-(const Vector3& vec) const
		{
			return Vector3(x-vec.x, y-vec.y, z-vec.z);
		}

		//////////////////////////////////////////////
		// scales the given vector by the provided amount and adds it to this one.
		// Useful in some calculations and to keep code tidy
		//////////////////////////////////////////////
		void addScaledVector(const Vector3& vec, real scaleFactor)
		{
			x += vec.x * scaleFactor;
			y += vec.y * scaleFactor;
			z += vec.z * scaleFactor;
		}

		//////////////////////////////////////////////
		// performs a component product of the two vectors i.e.
		// x = x1 * x2 etc. and returns the result
		//////////////////////////////////////////////
		Vector3 componentProduct(const Vector3 &vec) const
		{
			return Vector3(x * vec.x, y * vec.y, z * vec.z);
		}

		//////////////////////////////////////////////
		// performs a component product of two vectors and replaces
		// the vector it was applied to
		//////////////////////////////////////////////
		void componentProductUpdate(const Vector3 &vec)
		{
			x *= vec.x;
			y *= vec.y;
			z *= vec.z;
		}

		//////////////////////////////////////////////
		// performs a dot product of the two vectors and returns the value
		//////////////////////////////////////////////
		real dotProduct(const Vector3 &vec) const
		{
			return (x*vec.x + y*vec.y + z*vec.z);
		}

		//////////////////////////////////////////////
		// Overload * of two vectors to do a dot product
		//////////////////////////////////////////////
		real operator*(const Vector3 &vec) const
		{
			return (x*vec.x + y*vec.y + z*vec.z);
		}

		/////////////////////////////////////////////
		// performs a cross product of the two vectors and returns the vector
		//////////////////////////////////////////////
		Vector3 crossProduct(const Vector3 &vec) const
		{
			return Vector3(	y*vec.z - z*vec.y,
							z*vec.x - x*vec.z,
							x*vec.y - y*vec.x);
		}

		//////////////////////////////////////////////
		// Overload % of two vectors to do a cross product and returns the vector
		//////////////////////////////////////////////
		Vector3 operator%(const Vector3 &vec) const
		{
			return Vector3(	y*vec.z - z*vec.y,
							z*vec.x - x*vec.z,
							x*vec.y - y*vec.x);
		}

		//////////////////////////////////////////////
		// Overload %= of two vectors to do a cross product and replaces this object
		//////////////////////////////////////////////
		void operator%=(const Vector3 &vec)
		{
			*this = crossProduct(vec);
		}

		//////////////////////////////////////////////
		// Overload ==
		//////////////////////////////////////////////
		bool operator==(const Vector3& vec) const
		{
			return	x == vec.x && 
					y == vec.y &&
					z == vec.z;
		}
		//////////////////////////////////////////////
		// Overload != (use == as a shortcut)
		//////////////////////////////////////////////
		bool operator!=(const Vector3 &vec) const
		{
			return !(*this == vec);
		}

		//////////////////////////////////////////////
		// Overload [] to provide quick access to components
		//////////////////////////////////////////////
		real operator[](unsigned i) const
		{
			if (i == 0) return x;
			if (i == 1) return y;
			return z; // return z in all other cases
		}

		//////////////////////////////////////////////
		// Overload [], non-const version
		real& operator[](unsigned i)
		{
			if (i == 0) return x;
			if (i == 1) return y;
			return z;
		}
		
		//////////////////////////////////////////////
		// clears out all data from the vector (sets everything to zero)
		//////////////////////////////////////////////
		void clear(void)
		{
			x = y = z = 0;
		}

		//////////////////////////////////////////////
		// trims the length of the vector to be at most
		// as large as the provided size. Keeps the 
		// directional information. Useful to scale
		// vectors down to a maximum size if necessary
		//////////////////////////////////////////////
		void trim(real size)
		{
			if (squareMagnitude() > size*size)
			{
				normalize();
				x *= size;
				y *= size;
				z *= size;
			}
		}

		// some constant declarations
		const static Vector3 UP;
        const static Vector3 DOWN;
		const static Vector3 LEFT;
		const static Vector3 RIGHT;
		
	};

	//////////////////////////////////////////////
    // Holds a three degrees of freedom for orientation.
    // Quaternions have several mathematical properties that make them 
	// useful for representing orientations, but require four items of data to
    // hold the three degrees of freedom. These four items of data can
    // be viewed as the coefficients of a complex number with three
    // imaginary parts. The mathematics of the quaternion is then
    // defined and is roughly correspondent to the math of 3D
    // rotations. A quaternion is only a valid rotation if it is
    // normalised: i.e. it has a length of 1.
     
    // @note Angular velocity and acceleration can be correctly
    // represented as vectors. Quaternions are only needed for
    // orientation.
	//////////////////////////////////////////////
     
    class Quaternion
    {
    public:
        union {

			// use a union so we can hold the same data in two different ways

            struct {
                
				real r;		// real component
				real i;		// first imaginary component
				real j;		// second imaginary component
				real k;		// third imaginary component
            };

            real data[4];	// same data in array form
        };


        // default Quarternion. Zero rotation.
        Quaternion() : r(1), i(0), j(0), k(0) {}

        // Note: Quaternion needs to be normalized if the given values
		// are to be a valid rotation
        Quaternion(const real r, const real i, const real j, const real k)
            : r(r), i(i), j(j), k(k)
        {
        }

        // normalizes the Quarternion to a valid length so it can be used.
        void normalize()
        {
            real d = r*r+i*i+j*j+k*k;

            // Check for zero length quaternion, and use the no-rotation
            // quaternion in that case.
            if (d == 0) {
                r = 1;
                return;
            }

            d = ((real)1.0)/real_sqrt(d);
            r *= d;
            i *= d;
            j *= d;
            k *= d;
        }

        // multiplies quarternions
        void operator *=(const Quaternion &multiplier)
        {
            Quaternion q = *this;
            r = q.r*multiplier.r - q.i*multiplier.i -
                q.j*multiplier.j - q.k*multiplier.k;
            i = q.r*multiplier.i + q.i*multiplier.r +
                q.j*multiplier.k - q.k*multiplier.j;
            j = q.r*multiplier.j + q.j*multiplier.r +
                q.k*multiplier.i - q.i*multiplier.k;
            k = q.r*multiplier.k + q.k*multiplier.r +
                q.i*multiplier.j - q.j*multiplier.i;
        }

        // adds a scaled vector to this quarternion. Used to update
		// an orientation by a vector wrt time
        void addScaledVector(const Vector3& vector, real scale)
        {
            Quaternion q(0,
                vector.x * scale,
                vector.y * scale,
                vector.z * scale);
            q *= *this;
            r += q.r * ((real)0.5);
            i += q.i * ((real)0.5);
            j += q.j * ((real)0.5);
            k += q.k * ((real)0.5);
        }

		// rotate this quarternion by a vector
        void rotateByVector(const Vector3& vector)
        {
            Quaternion q(0, vector.x, vector.y, vector.z);
            (*this) *= q;
        }
    };

    // transformation/rotation/scaling matrix type
    class Matrix4
    {
    public:
		
        real data[12]; //data. Note that we skip using the last 4 elements.
		// these are unnecessary since they are always 0, 0, 0, 1


        // constructor makes an identity matrix
        Matrix4()
        {
            data[1] = data[2] = data[3] = data[4] = data[6] =
                data[7] = data[8] = data[9] = data[11] = 0;
            data[0] = data[5] = data[10] = 1;
        }

        // diagonal matrix
        void setDiagonal(real a, real b, real c)
        {
            data[0] = a;
            data[5] = b;
            data[10] = c;
        }

        // matrix multiplication
        Matrix4 operator*(const Matrix4 &o) const
        {
            Matrix4 result;
            result.data[0] = (o.data[0]*data[0]) + (o.data[4]*data[1]) + (o.data[8]*data[2]);
            result.data[4] = (o.data[0]*data[4]) + (o.data[4]*data[5]) + (o.data[8]*data[6]);
            result.data[8] = (o.data[0]*data[8]) + (o.data[4]*data[9]) + (o.data[8]*data[10]);

            result.data[1] = (o.data[1]*data[0]) + (o.data[5]*data[1]) + (o.data[9]*data[2]);
            result.data[5] = (o.data[1]*data[4]) + (o.data[5]*data[5]) + (o.data[9]*data[6]);
            result.data[9] = (o.data[1]*data[8]) + (o.data[5]*data[9]) + (o.data[9]*data[10]);

            result.data[2] = (o.data[2]*data[0]) + (o.data[6]*data[1]) + (o.data[10]*data[2]);
            result.data[6] = (o.data[2]*data[4]) + (o.data[6]*data[5]) + (o.data[10]*data[6]);
            result.data[10] = (o.data[2]*data[8]) + (o.data[6]*data[9]) + (o.data[10]*data[10]);

            result.data[3] = (o.data[3]*data[0]) + (o.data[7]*data[1]) + (o.data[11]*data[2]) + data[3];
            result.data[7] = (o.data[3]*data[4]) + (o.data[7]*data[5]) + (o.data[11]*data[6]) + data[7];
            result.data[11] = (o.data[3]*data[8]) + (o.data[7]*data[9]) + (o.data[11]*data[10]) + data[11];

            return result;
        }

        // transforms a vector by this matrix (note we assume the vector
		// has a 4th w-component of 1, so that we can multiply a Vector3
		// by a Matrix4 in the typical mathematical sense)
        Vector3 operator*(const Vector3 &vector) const
        {
            return Vector3(
                vector.x * data[0] +
                vector.y * data[1] +
                vector.z * data[2] + data[3],

                vector.x * data[4] +
                vector.y * data[5] +
                vector.z * data[6] + data[7],

                vector.x * data[8] +
                vector.y * data[9] +
                vector.z * data[10] + data[11]
            );
        }

        // transforms the vector by the matrix
        Vector3 transform(const Vector3 &vector) const
        {
            return (*this) * vector;
        }

        // get the determinant of the matrix
        real getDeterminant() const;

        // sets this matrix to its inverse
        void setInverse(const Matrix4 &m);

        // returns the matrix inverse
        Matrix4 inverse() const
        {
            Matrix4 result;
            result.setInverse(*this);
            return result;
        }

        // inverts this matrix
        void invert()
        {
            setInverse(*this);
        }

        // transform the vector by this matrix
        Vector3 transformDirection(const Vector3 &vector) const
        {
            return Vector3(
                vector.x * data[0] +
                vector.y * data[1] +
                vector.z * data[2],

                vector.x * data[4] +
                vector.y * data[5] +
                vector.z * data[6],

                vector.x * data[8] +
                vector.y * data[9] +
                vector.z * data[10]
            );
        }

        // transforms the vector by the matrix's inverse. If it contains scale
		// or shear, this function will not work.
        Vector3 transformInverseDirection(const Vector3 &vector) const
        {
            return Vector3(
                vector.x * data[0] +
                vector.y * data[4] +
                vector.z * data[8],

                vector.x * data[1] +
                vector.y * data[5] +
                vector.z * data[9],

                vector.x * data[2] +
                vector.y * data[6] +
                vector.z * data[10]
            );
        }

        // as above, transforms the vector by the matrix inverse. Will not worth
		// with scale/shear matrices.
        Vector3 transformInverse(const Vector3 &vector) const
        {
            Vector3 tmp = vector;
            tmp.x -= data[3];
            tmp.y -= data[7];
            tmp.z -= data[11];
            return Vector3(
                tmp.x * data[0] +
                tmp.y * data[4] +
                tmp.z * data[8],

                tmp.x * data[1] +
                tmp.y * data[5] +
                tmp.z * data[9],

                tmp.x * data[2] +
                tmp.y * data[6] +
                tmp.z * data[10]
            );
        }

        // get one column of the matrix
        Vector3 getAxisVector(int i) const
        {
            return Vector3(data[i], data[i+4], data[i+8]);
        }

        // create a rotation matrix from the given quarternion
        void setOrientationAndPos(const Quaternion &q, const Vector3 &pos)
        {
            data[0] = 1 - (2*q.j*q.j + 2*q.k*q.k);
            data[1] = 2*q.i*q.j + 2*q.k*q.r;
            data[2] = 2*q.i*q.k - 2*q.j*q.r;
            data[3] = pos.x;

            data[4] = 2*q.i*q.j - 2*q.k*q.r;
            data[5] = 1 - (2*q.i*q.i  + 2*q.k*q.k);
            data[6] = 2*q.j*q.k + 2*q.i*q.r;
            data[7] = pos.y;

            data[8] = 2*q.i*q.k + 2*q.j*q.r;
            data[9] = 2*q.j*q.k - 2*q.i*q.r;
            data[10] = 1 - (2*q.i*q.i  + 2*q.j*q.j);
            data[11] = pos.z;
        }

        // fill data from an OpenGL array. Not used in DX.
        void fillGLArray(float array[16]) const
        {
            array[0] = (float)data[0];
            array[1] = (float)data[4];
            array[2] = (float)data[8];
            array[3] = (float)0;

            array[4] = (float)data[1];
            array[5] = (float)data[5];
            array[6] = (float)data[9];
            array[7] = (float)0;

            array[8] = (float)data[2];
            array[9] = (float)data[6];
            array[10] = (float)data[10];
            array[11] = (float)0;

            array[12] = (float)data[3];
            array[13] = (float)data[7];
            array[14] = (float)data[11];
            array[15] = (float)1;
        }

		D3DXMATRIX ConvertToDX()
		{
			D3DXMATRIX w;

			w._11 = (float)data[0];
			w._12 = (float)data[4];
			w._13 = (float)data[8];
			w._14 = 0;

			w._21 = (float)data[1];
			w._22 = (float)data[5];
			w._23 = (float)data[9];
			w._24 = 0;

			w._31 = (float)data[2];
			w._32 = (float)data[6];
			w._33 = (float)data[10];
			w._34 = 0;

			w._41 = (float)data[3];
			w._42 = (float)data[7];
			w._43 = (float)data[11];
			w._44 = 1;

			return w;
		}
    };

	/**
     * Holds an inertia tensor, consisting of a 3x3 row-major matrix.
     * This matrix is not padding to produce an aligned structure, since
     * it is most commonly used with a mass (single real) and two
     * damping coefficients to make the 12-element characteristics array
     * of a rigid body.
     */
	class Matrix3
	{
	public:
		// matrix data in array form
		real data[9];

		// constructor
		Matrix3()
		{
			data[0] = 
				data[1] =
				data[2] =
				data[3] =
				data[4] =
				data[5] =
				data[6] =
				data[7] =
				data[8] = 0;
		}

		// creates a new matrix from the three given vectors
        Matrix3(const Vector3 &compOne, const Vector3 &compTwo,
            const Vector3 &compThree)
        {
            setComponents(compOne, compTwo, compThree);
        }

        // create a new matrix with explicit values
        Matrix3(real c0, real c1, real c2, real c3, real c4, real c5,
            real c6, real c7, real c8)
        {
            data[0] = c0; data[1] = c1; data[2] = c2;
            data[3] = c3; data[4] = c4; data[5] = c5;
            data[6] = c6; data[7] = c7; data[8] = c8;
        }

        // create a diagonal matrix using the 3 given values
        void setDiagonal(real a, real b, real c)
        {
            setInertiaTensorCoeffs(a, b, c);
        }

        // creates an intertia tensor based matrix
        void setInertiaTensorCoeffs(real ix, real iy, real iz,
            real ixy=0, real ixz=0, real iyz=0)
        {
            data[0] = ix;
            data[1] = data[3] = -ixy;
            data[2] = data[6] = -ixz;
            data[4] = iy;
            data[5] = data[7] = -iyz;
            data[8] = iz;
        }

        // creates the inertia tensor of a rectangular block
        void setBlockInertiaTensor(const Vector3 &halfSizes, real mass)
        {
            Vector3 squares = halfSizes.componentProduct(halfSizes);
            setInertiaTensorCoeffs(0.3f*mass*(squares.y + squares.z),
                0.3f*mass*(squares.x + squares.z),
                0.3f*mass*(squares.x + squares.y));
        }

        // creates a skew matrix
        void setSkewSymmetric(const Vector3 vector)
        {
            data[0] = data[4] = data[8] = 0;
            data[1] = -vector.z;
            data[2] = vector.y;
            data[3] = vector.z;
            data[5] = -vector.x;
            data[6] = -vector.y;
            data[7] = vector.x;
        }

        // sets the components of a matrix from the given vectors
        void setComponents(const Vector3 &compOne, const Vector3 &compTwo,
            const Vector3 &compThree)
        {
            data[0] = compOne.x;
            data[1] = compTwo.x;
            data[2] = compThree.x;
            data[3] = compOne.y;
            data[4] = compTwo.y;
            data[5] = compThree.y;
            data[6] = compOne.z;
            data[7] = compTwo.z;
            data[8] = compThree.z;

        }

        // multiply a vector by a matrix to get an output vector
        Vector3 operator*(const Vector3 &vector) const
        {
            return Vector3(
                vector.x * data[0] + vector.y * data[1] + vector.z * data[2],
                vector.x * data[3] + vector.y * data[4] + vector.z * data[5],
                vector.x * data[6] + vector.y * data[7] + vector.z * data[8]
            );
        }

        // another way to multiply
        Vector3 transform(const Vector3 &vector) const
        {
            return (*this) * vector;
        }

        // transform the vector by the matrix transpose
        Vector3 transformTranspose(const Vector3 &vector) const
        {
            return Vector3(
                vector.x * data[0] + vector.y * data[3] + vector.z * data[6],
                vector.x * data[1] + vector.y * data[4] + vector.z * data[7],
                vector.x * data[2] + vector.y * data[5] + vector.z * data[8]
            );
        }

        // get a vector from the given row id
        Vector3 getRowVector(int i) const
        {
            return Vector3(data[i*3], data[i*3+1], data[i*3+2]);
        }

        // get a vector from the given axis id
        Vector3 getAxisVector(int i) const
        {
            return Vector3(data[i], data[i+3], data[i+6]);
        }

        // sets the matrix to the inverse of the given matrix
        void setInverse(const Matrix3 &m)
        {
            real t4 = m.data[0]*m.data[4];
            real t6 = m.data[0]*m.data[5];
            real t8 = m.data[1]*m.data[3];
            real t10 = m.data[2]*m.data[3];
            real t12 = m.data[1]*m.data[6];
            real t14 = m.data[2]*m.data[6];

            // Calculate the determinant
            real t16 = (t4*m.data[8] - t6*m.data[7] - t8*m.data[8]+
                        t10*m.data[7] + t12*m.data[5] - t14*m.data[4]);

            // Make sure the determinant is non-zero.
            if (t16 == (real)0.0f) return;
            real t17 = 1/t16;

            data[0] = (m.data[4]*m.data[8]-m.data[5]*m.data[7])*t17;
            data[1] = -(m.data[1]*m.data[8]-m.data[2]*m.data[7])*t17;
            data[2] = (m.data[1]*m.data[5]-m.data[2]*m.data[4])*t17;
            data[3] = -(m.data[3]*m.data[8]-m.data[5]*m.data[6])*t17;
            data[4] = (m.data[0]*m.data[8]-t14)*t17;
            data[5] = -(t6-t10)*t17;
            data[6] = (m.data[3]*m.data[7]-m.data[4]*m.data[6])*t17;
            data[7] = -(m.data[0]*m.data[7]-t12)*t17;
            data[8] = (t4-t8)*t17;
        }

        // returns the inverse of this matrix
        Matrix3 inverse() const
        {
            Matrix3 result;
            result.setInverse(*this);
            return result;
        }

        // inverts this matrix
        void invert()
        {
            setInverse(*this);
        }

        // sets the given matrix to the tranpose of this matrix
        void setTranspose(const Matrix3 &m)
        {
            data[0] = m.data[0];
            data[1] = m.data[3];
            data[2] = m.data[6];
            data[3] = m.data[1];
            data[4] = m.data[4];
            data[5] = m.data[7];
            data[6] = m.data[2];
            data[7] = m.data[5];
            data[8] = m.data[8];
        }

        // returns the transpose of this matrix
        Matrix3 transpose() const
        {
            Matrix3 result;
            result.setTranspose(*this);
            return result;
        }

        // returns a matrix multiple
        Matrix3 operator*(const Matrix3 &o) const
        {
            return Matrix3(
                data[0]*o.data[0] + data[1]*o.data[3] + data[2]*o.data[6],
                data[0]*o.data[1] + data[1]*o.data[4] + data[2]*o.data[7],
                data[0]*o.data[2] + data[1]*o.data[5] + data[2]*o.data[8],

                data[3]*o.data[0] + data[4]*o.data[3] + data[5]*o.data[6],
                data[3]*o.data[1] + data[4]*o.data[4] + data[5]*o.data[7],
                data[3]*o.data[2] + data[4]*o.data[5] + data[5]*o.data[8],

                data[6]*o.data[0] + data[7]*o.data[3] + data[8]*o.data[6],
                data[6]*o.data[1] + data[7]*o.data[4] + data[8]*o.data[7],
                data[6]*o.data[2] + data[7]*o.data[5] + data[8]*o.data[8]
                );
        }

        // matrix multiple and assignment
        void operator*=(const Matrix3 &o)
        {
            real t1;
            real t2;
            real t3;

            t1 = data[0]*o.data[0] + data[1]*o.data[3] + data[2]*o.data[6];
            t2 = data[0]*o.data[1] + data[1]*o.data[4] + data[2]*o.data[7];
            t3 = data[0]*o.data[2] + data[1]*o.data[5] + data[2]*o.data[8];
            data[0] = t1;
            data[1] = t2;
            data[2] = t3;

            t1 = data[3]*o.data[0] + data[4]*o.data[3] + data[5]*o.data[6];
            t2 = data[3]*o.data[1] + data[4]*o.data[4] + data[5]*o.data[7];
            t3 = data[3]*o.data[2] + data[4]*o.data[5] + data[5]*o.data[8];
            data[3] = t1;
            data[4] = t2;
            data[5] = t3;

            t1 = data[6]*o.data[0] + data[7]*o.data[3] + data[8]*o.data[6];
            t2 = data[6]*o.data[1] + data[7]*o.data[4] + data[8]*o.data[7];
            t3 = data[6]*o.data[2] + data[7]*o.data[5] + data[8]*o.data[8];
            data[6] = t1;
            data[7] = t2;
            data[8] = t3;
        }

        // multiplies the matrix by the scalar
        void operator*=(const real scalar)
        {
            data[0] *= scalar; data[1] *= scalar; data[2] *= scalar;
            data[3] *= scalar; data[4] *= scalar; data[5] *= scalar;
            data[6] *= scalar; data[7] *= scalar; data[8] *= scalar;
        }

        // component-wise addition of two matrices
        void operator+=(const Matrix3 &o)
        {
            data[0] += o.data[0]; data[1] += o.data[1]; data[2] += o.data[2];
            data[3] += o.data[3]; data[4] += o.data[4]; data[5] += o.data[5];
            data[6] += o.data[6]; data[7] += o.data[7]; data[8] += o.data[8];
        }

        // sets the matrix to be a rotation matrix of
		// the given Quarternion
        void setOrientation(const Quaternion &q)
        {
            data[0] = 1 - (2*q.j*q.j + 2*q.k*q.k);
            data[1] = 2*q.i*q.j + 2*q.k*q.r;
            data[2] = 2*q.i*q.k - 2*q.j*q.r;
            data[3] = 2*q.i*q.j - 2*q.k*q.r;
            data[4] = 1 - (2*q.i*q.i  + 2*q.k*q.k);
            data[5] = 2*q.j*q.k + 2*q.i*q.r;
            data[6] = 2*q.i*q.k + 2*q.j*q.r;
            data[7] = 2*q.j*q.k - 2*q.i*q.r;
            data[8] = 1 - (2*q.i*q.i  + 2*q.j*q.j);
        }

		D3DXMATRIX ConvertToDX()
		{
			D3DXMATRIX w;

			w._11 = (float)data[0];
			w._12 = (float)data[3];
			w._13 = (float)data[6];
			w._14 = 0;

			w._21 = (float)data[1];
			w._22 = (float)data[4];
			w._23 = (float)data[7];
			w._24 = 0;

			w._31 = (float)data[2];
			w._32 = (float)data[5];
			w._33 = (float)data[8];
			w._34 = 0;

			w._41 = 0;
			w._42 = 0;
			w._43 = 0;
			w._44 = 1;

			return w;
		}

        // interpolates a couple of matrices
        static Matrix3 linearInterpolate(const Matrix3& a, const Matrix3& b, real prop);
    };
}

